#!/bin/bash
lines=116


echo "解压安装包"
if [ -d "./nodes" ] #判定是否存在老的程序目录如是则删除
then
rm -fr nodes
fi
tail -n +$lines "$0" > nodes.tar.gz #提取程序
#xz -d nodes.tar.xz
tar -xvf nodes.tar.gz #解压程序

echo "拷贝节点到ros2_ws/install"
if [ ! -d ~/ros2_ws/install/ ]
then
mkdir -p ~/ros2_ws/install/ #创建程序存放目录
fi

cp -fr ./nodes/install/* ~/ros2_ws/install/ #拷贝程序
echo "拷贝树"
if [ ! -d ~/ros2_ws/tree/ ]
then
mkdir -p ~/ros2_ws/tree/ #创建行为树xml存放目录
fi
cp -fr ./nodes/tree/* ~/ros2_ws/tree/ #拷贝行为数xml文件

echo "拷贝DDS2ROS"
if [ ! -d ~/ros2_ws/install/dds2ros/ ]
then
mkdir -p ~/ros2_ws/install/dds2ros/ #创建DDS2ROS程序存放目录
fi
cp ./nodes/DDS2ROS ~/ros2_ws/install/dds2ros/ #拷贝DDS2ROS程序
cp ./nodes/dds_profiles.xml ~/ros2_ws/install/dds2ros/ #拷贝配置文件


if [ ! -d ~/droneyee_fly/ ]
then
echo "创建启动脚本路径"
mkdir -p ~/droneyee_fly
fi
if [ ! -f ~/droneyee_fly/realfly_startup.sh ]
then
echo "拷贝启动脚本"
cp ./nodes/realfly_startup.sh ~/droneyee_fly/realfly_startup.sh #拷贝全部程序的启动脚本（同时此脚本为开机启动的脚本）
cp ./nodes/check_restart.sh ~/droneyee_fly/check_restart.sh
cp ./nodes/max_gpu_freq.sh ~/droneyee_fly/max_gpu_freq.sh 
cp ./nodes/max_cpu_freq.sh ~/droneyee_fly/max_cpu_freq.sh 
fi
#拷贝mavros参数文件
if [ ! -f ~/ros2_ws/mavros_param_1.yaml ]
then
echo "拷贝mavros参数文件"
cp ./nodes/mavros_param_1.yaml ~/ros2_ws/mavros_param_1.yaml
fi

#删除安装包中解压内容及压缩包
rm -fr nodes
rm nodes.tar.gz
#修改.bashrc中的环境变量
addSource(){
src=$(grep -i "${1}" ~/.bashrc)
len=$(expr length "$src")
if [ $len -eq 0 ] 
then
echo "source ${1}" | tee -a ~/.bashrc
fi
}

vehiId=1
read -p "输入飞机id:" vehiId


if [ $vehiId -le 0 -o $vehiId -gt 254 ]
#id值非法时设置为1
then
vehiId = 1
fi

preId="tgt_system: 1"
#依据输入id替换mavros中的id
echo "preId: ${preId}"
#
newId="tgt_system: ${vehiId}"
echo "newId: ${newId}"
sed -i "s/${preId}/${newId}/g" ~/ros2_ws/mavros_param_1.yaml
#sed -i "s/${preId}/${newId}/g" ~/ros2_ws/install/mavros/share/mavros/launch/px4.launch
#sed -i 's/"tgt_system" default="1"/"tgt_system" default="$vehiId"/g' ~/ros2_ws/install/mavros/share/mavros/launch/px4.launch


#更新domainid
domainInf=$(grep -i "ROS_DOMAIN" ~/.bashrc)
echo "domainInf: ${domainInf}"
newDomain="export ROS_DOMAIN_ID=${vehiId}"
len=$(expr length "$domainInf")
if [ $len -eq 0 ]

then
echo "增加ROS_DOMAIN_ID"
echo "${newDomain}" | tee -a ~/.bashrc
else
echo "替换ROS_DOMAIN_ID"
sed -i "s/${domainInf}/${newDomain}/g" ~/.bashrc
fi

#增加source
addSource "/opt/ros/foxy/setup.bash"
addSource "~/ros2_ws/install/setup.bash"

#echo "增加开机启动"
#gnome-terminal -x ~/droneyee_fly/realfly_noterminal_startup.sh

#rm nodes.tar.xz
echo "安装完成"
exit 0
